clear all;
close all;

Qk=[0.1;0;0.4;0.5];
Zk=[0;0];
L2 = [ -54.8247 63.0280 0 -24.7515;12.3734 -7.8000 0 4.2552];

T=0.001;
for k=1:1:30000

    time(k)=k*T;
    
    if time(k)>=5
        dt1 = k*T*sin(1/(k*T));
        dt2 = 1/(k*T+1) + 0.5;
    else
        dt1 = 0;
        dt2 = 0;
    end
%     dt1 = 0.1*(k*T*sin(1/(k*T)));
%     dt2 = 0.1*(1/(k*T+1) + 0.5);
    dt(:,k)=[dt1;dt2];
    
    x1=Qk(1);x2=Qk(2);x3=Qk(3);x4=Qk(4);
    zt(:,k)=[x1;x2;x3;x4];
    d=Zk+L2*zt(:,k);
    d1=d(1);d2=d(2);
    ddt(:,k)=[d1;d2];

    s1 = 0.1*(4*d2 + 10*x3 + 2*d1 + 25*x2); 
    s2 = 0.1*(2*d1 + 5*x2 + 10*x4 + 20*x1 + 3*d2);
    s(:,k)=[s1;s2];

    u1 = -2.5*x3 - x4 - 0.5*d1 - d2 - 1.5*sign(s1);
    u2 = 6*x1 + 0.92*x2 + 0.7*x3 - 3*x4 - 0.5*d1 - 1.2*d2 - 1.5*sign(s2);
    ut(:,k)=[u1;u2];

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,ut(:,k),dt(:,k)),tSpan,Qk);
    Qk = Qq(length(Qq),:);
	[tt1,Zq] = ode45(@(t,x) obv(t,x,zt(:,k),ut(:,k)),tSpan,Zk);
    Zk = Zq(length(Zq),:);
     
end

figure(1);
plot(time,zt(1,:),'k',time,zt(2,:),'r',time,zt(3,:),'b',time,zt(4,:),'g');
xlabel('time(s)');ylabel('th');
legend('x1','x2','x3','x4');
grid on;

figure(2);
plot(time,ut(1,:),'k',time,ut(2,:),'r');
xlabel('time(s)');ylabel('U');
legend('U1','U2');
 
figure(3);
plot(time,dt(1,:),'k',time,ddt(1,:),'r');
xlabel('time(s)');ylabel('edt');
legend('dt1','ddt1');
figure(4);
plot(time,dt(2,:),'k',time,ddt(2,:),'r');
xlabel('time(s)');ylabel('edt');
legend('dt2','ddt2');
